{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "running_as_notebook = False  # Manually set this to True if you are a human (see #13862).\n",
    "\n",
    "# Imports\n",
    "import numpy as np\n",
    "\n",
    "from IPython.display import display\n",
    "from ipywidgets import Layout, Text, ToggleButton\n",
    "\n",
    "from pydrake.geometry import DrakeVisualizer, SceneGraph\n",
    "from pydrake.lcm import DrakeLcm\n",
    "from pydrake.multibody.plant import AddMultibodyPlantSceneGraph, MultibodyPlant\n",
    "from pydrake.multibody.parsing import Parser\n",
    "from pydrake.systems.analysis import Simulator                                      \n",
    "from pydrake.systems.framework import DiagramBuilder\n",
    "from pydrake.systems.rendering import MultibodyPositionToGeometryPose\n",
    "\n",
    "from pydrake.multibody.jupyter_widgets import JointSliders, MakeJointSlidersThatPublishOnCallback"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# JointSliders\n",
    "\n",
    "This version of the sliders is intended for use as a System in a Diagram.\n",
    "\n",
    "Running this cell should allow you to control the joints of the IIWA in Meldis.  You must set `running_as_notebook` to `True` in the cell above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "builder = DiagramBuilder()\n",
    "\n",
    "# Note: Don't use AddMultibodyPlantSceneGraph because we are only using\n",
    "# MultibodyPlant for parsing, then wiring our sliders directly to SceneGraph.\n",
    "scene_graph = builder.AddSystem(SceneGraph())\n",
    "plant = MultibodyPlant(time_step=0.0)\n",
    "plant.RegisterAsSourceForSceneGraph(scene_graph)\n",
    "Parser(plant).AddModelsFromUrl(\n",
    "    url=\"package://drake_models/iiwa_description/sdf/iiwa7_no_collision.sdf\")\n",
    "plant.Finalize()\n",
    "\n",
    "# Add sliders to set positions of the joints.\n",
    "sliders = builder.AddSystem(JointSliders(robot=plant))\n",
    "to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))\n",
    "builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())\n",
    "builder.Connect(\n",
    "    to_pose.get_output_port(),\n",
    "    scene_graph.get_source_pose_port(plant.get_source_id()))\n",
    "\n",
    "# Note: We can't use AddDefaultVisualization because there is no plant\n",
    "# in the Diagram.\n",
    "DrakeVisualizer.AddToBuilder(builder, scene_graph)\n",
    "\n",
    "# Make the diagram and run it.\n",
    "diagram = builder.Build()\n",
    "simulator = Simulator(diagram)\n",
    "\n",
    "if running_as_notebook:  \n",
    "    simulator.set_target_realtime_rate(1.0)\n",
    "    stop_button = ToggleButton(value=False, description='Stop Simulation')\n",
    "    display(stop_button)\n",
    "    while not stop_button.value:\n",
    "        simulator.AdvanceTo(simulator.get_context().get_time() + 5.0)\n",
    "    stop_button.value = False\n",
    "else:  # running as a test.\n",
    "    simulator.AdvanceTo(0.1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# MakeJointSlidersThatPublishOnCallback\n",
    "\n",
    "This version of the sliders uses callback on the widget events to call Publish.  It does not provide a System and does not use Simulator.  While it doesn't work for as many applications, it has the advantage that one does not have to `AdvanceTo` in a loop to have a meaningful interaction.\n",
    "\n",
    "Running this cell should allow you to control the joints of the IIWA in Meldis.  This time, it is without a simulator and relying solely on callbacks.\n",
    "\n",
    "You should see the output in the textbox below the sliders update with the number of times the callback is called; it should report that it was called once immediately, before you even move the sliders."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "builder = DiagramBuilder()\n",
    "plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)\n",
    "Parser(builder).AddModelsFromUrl(\n",
    "    url=\"package://drake_models/iiwa_description/sdf/iiwa7_no_collision.sdf\")\n",
    "plant.Finalize()\n",
    "\n",
    "viz = DrakeVisualizer.AddToBuilder(builder, scene_graph)\n",
    "\n",
    "# Make the diagram and run it.\n",
    "diagram = builder.Build()\n",
    "context = diagram.CreateDefaultContext()\n",
    "\n",
    "callback_output = Text(value=\"Not called yet\", description=\"Callback output: \", \n",
    "                       layout=Layout(width='500px'), \n",
    "                       style={'description_width':'initial'})\n",
    "num_calls = 0\n",
    "def callback_test(context):\n",
    "    global num_calls\n",
    "    num_calls += 1\n",
    "    callback_output.value = f\"Called {num_calls} times\"\n",
    "\n",
    "sliders = MakeJointSlidersThatPublishOnCallback(plant, viz, context, my_callback=callback_test)\n",
    "display(callback_output)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
